#include "daf_user_control.h"
#include "../agent/xyd_manager.h"
#include "gazella/logger.h"
#include <mavsdk/plugins/manual_control/manual_control_impl.h>
#include <mavsdk/plugins/manual_control/manual_control.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry_impl.h>
#include "../agent/connect/connect_manager.h"

namespace Gazella
{
    namespace Daf
    {
        DafUserControl::DafUserControl()
        {
        }

        DafUserControl::~DafUserControl()
        {
        }

        DafUserControl *DafUserControl::get_instance()
        {
            static DafUserControl instance;

            return &instance;
        }

        // auto generate, don't modify
        void DafUserControl::control_notify(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {

            if (cmd.identifier() == "lockCtl")
            {
                __control_notify__lockCtl(cmd, ack);
            }

            if (cmd.identifier() == "takeoff")
            {
                __control_notify__takeoff(cmd, ack);
            }

            if (cmd.identifier() == "return")
            {
                __control_notify__return(cmd, ack);
            }

            if (cmd.identifier() == "land")
            {
                __control_notify__land(cmd, ack);
            }

            if (cmd.identifier() == "hover")
            {
                __control_notify__hover(cmd, ack);
            }

            if (cmd.identifier() == "flyTo")
            {
                __control_notify__flyTo(cmd, ack);
            }

            if (cmd.identifier() == "speedM")
            {
                __control_notify__speedM(cmd, ack);
            }

            if (cmd.identifier() == "ctlJoy")
            {
                __control_notify__ctlJoy(cmd, ack);
            }

            if (cmd.identifier() == "navFly")
            {
                __control_notify__navFly(cmd, ack);
            }

            if (cmd.identifier() == "resumeNav")
            {
                __control_notify__resumeNav(cmd, ack);
            }

            if (cmd.identifier() == "stopNav")
            {
                __control_notify__stopNav(cmd, ack);
            }

            if (cmd.identifier() == "takePhoto")
            {
                __control_notify__takePhoto(cmd, ack);
            }

            if (cmd.identifier() == "startVideo")
            {
                __control_notify__startVideo(cmd, ack);
            }

            if (cmd.identifier() == "stopVideo")
            {
                __control_notify__stopVideo(cmd, ack);
            }

            if (cmd.identifier() == "zoomIn")
            {
                __control_notify__zoomIn(cmd, ack);
            }

            if (cmd.identifier() == "zoomOut")
            {
                __control_notify__zoomOut(cmd, ack);
            }

            if (cmd.identifier() == "zoomScale")
            {
                __control_notify__zoomScale(cmd, ack);
            }

            if (cmd.identifier() == "gimbalFollow")
            {
                __control_notify__gimbalFollow(cmd, ack);
            }

            if (cmd.identifier() == "gimbalReset")
            {
                __control_notify__gimbalReset(cmd, ack);
            }

            if (cmd.identifier() == "gimbalFree")
            {
                __control_notify__gimbalFree(cmd, ack);
            }

            if (cmd.identifier() == "gimbalLeft")
            {
                __control_notify__gimbalLeft(cmd, ack);
            }

            if (cmd.identifier() == "gimbalRight")
            {
                __control_notify__gimbalRight(cmd, ack);
            }

            if (cmd.identifier() == "gimbalUp")
            {
                __control_notify__gimbalUp(cmd, ack);
            }

            if (cmd.identifier() == "gimbalDown")
            {
                __control_notify__gimbalDown(cmd, ack);
            }

            if (cmd.identifier() == "weaponSwitch")
            {
                __control_notify__weaponSwitch(cmd, ack);
            }

            if (cmd.identifier() == "sShot")
            {
                __control_notify__sShot(cmd, ack);
            }

            if (cmd.identifier() == "rShot")
            {
                __control_notify__rShot(cmd, ack);
            }

            if (cmd.identifier() == "bomb")
            {
                __control_notify__bomb(cmd, ack);
            }

            if (cmd.identifier() == "attack")
            {
                __control_notify__attack(cmd, ack);
            }

            if (cmd.identifier() == "attackTar")
            {
                __control_notify__attackTar(cmd, ack);
            }

            if (cmd.identifier() == "stopAuto")
            {
                __control_notify__stopAuto(cmd, ack);
            }

            if (cmd.identifier() == "startAuto")
            {
                __control_notify__startAuto(cmd, ack);
            }

            if (cmd.identifier() == "restartFly")
            {
                __control_notify__restartFly(cmd, ack);
            }

            if (cmd.identifier() == "stopFly")
            {
                __control_notify__stopFly(cmd, ack);
            }
        }

        // control notify functions

        // TODO lockCtl 目前是急停
        void DafUserControl::__control_notify__lockCtl(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            mp_log_info(DEFAULT_LOGGER, "__control_notify__engineEmergencyStop Start");
            Action::Result emergency_stop_result = XydAgent::XydManager::getInstance()->m_xyd_action->kill();
            mp_log_info(DEFAULT_LOGGER, "__control_notify__engineEmergencyStop result: %d",
                        static_cast<int>(emergency_stop_result));
        }

        // takeoff
        void DafUserControl::__control_notify__takeoff(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            mp_log_info(DEFAULT_LOGGER, "DafUserControl::__control_notify__takeoff Start");
            Action::Result takeoff_action_result = XydAgent::XydManager::getInstance()->m_xyd_action->takeoff();
            mp_log_info(DEFAULT_LOGGER, "DafUserControl::__control_notify__takeoff result: %d",
                        static_cast<int>(takeoff_action_result));
        }

        // return
        void DafUserControl::__control_notify__return(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            mp_log_info(DEFAULT_LOGGER, "DafUserControl::__control_notify__return Start");
            Action::Result return_result = XydAgent::XydManager::getInstance()->m_xyd_action->return_to_launch();
            mp_log_info(DEFAULT_LOGGER, "DafUserControl::__control_notify__return result: %d",
                        static_cast<int>(return_result));
        }

        // land
        void DafUserControl::__control_notify__land(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            mp_log_info(DEFAULT_LOGGER, "DafUserControl::__control_notify__land Start");
            Action::Result land_result = XydAgent::XydManager::getInstance()->m_xyd_action->land();
            mp_log_info(DEFAULT_LOGGER, "DafUserControl::__control_notify__land result: %d",
                        static_cast<int>(land_result));
        }

        // hover
        void DafUserControl::__control_notify__hover(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            mp_log_info(DEFAULT_LOGGER, "__control_notify__hover Start");
            ManualControl::Result start_position_ctrl_result = XydAgent::XydManager::getInstance()->m_xyd_manual_control->start_position_control();
            mp_log_info(DEFAULT_LOGGER, "__control_notify__hover result: %d",
                        static_cast<int>(start_position_ctrl_result));
        }

        // moveToLocation
        void DafUserControl::__control_notify__flyTo(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // moveBySpeed
        void DafUserControl::__control_notify__speedM(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // ctlJoy
        void DafUserControl::__control_notify__ctlJoy(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            mp_log_info(DEFAULT_LOGGER, "__control_notify__ctlJoy Start");
            // 俯仰角
            auto pitch = cmd.property_struct().members().at("y1").property_int().value();
            // 横滚角
            auto roll = cmd.property_struct().members().at("x1").property_int().value();
            // 航向角
            auto yaw = cmd.property_struct().members().at("x").property_int().value();
            // 油门
            auto throttle = cmd.property_struct().members().at("y").property_int().value();
            mp_log_info(
                DEFAULT_LOGGER,
                "__control_notify__ctlJoy input y1 >> pitch: %d, x1 >> roll: %d, x >> yaw: %d, y >> throttle: %d",
                static_cast<int>(pitch), static_cast<int>(roll), static_cast<int>(yaw), static_cast<int>(throttle));

            const float xyd_pitch = XydAgent::XydManager::scaleJoyValue(pitch, -1, 1);
            const float xyd_roll = XydAgent::XydManager::scaleJoyValue(roll, -1, 1);
            const float xyd_yaw = XydAgent::XydManager::scaleJoyValue(yaw, -1, 1);
            const float xyd_throttle = XydAgent::XydManager::scaleJoyValue(throttle, 0, 1);
            mp_log_info(
                DEFAULT_LOGGER,
                "__control_notify__ctlJoy output y1 >> pitch: %f, x1 >> roll: %f, x >> yaw: %f, y >> throttle: %f",
                xyd_pitch, xyd_roll, xyd_yaw, xyd_throttle);
            // 俯仰角,横滚角,航向角,油门
            // pitch, roll, throttle, yaw
            XydAgent::XydManager::getInstance()->joy_cache.xyd_pitch = xyd_pitch;
            XydAgent::XydManager::getInstance()->joy_cache.xyd_roll = xyd_roll;
            XydAgent::XydManager::getInstance()->joy_cache.xyd_throttle = xyd_throttle;
            XydAgent::XydManager::getInstance()->joy_cache.xyd_yaw = xyd_yaw;
            // TODO 注释原因
            auto result = XydAgent::XydManager::getInstance()->m_xyd_manual_control->set_manual_control_input(xyd_pitch, xyd_roll, xyd_throttle, xyd_yaw);
            mp_log_info(DEFAULT_LOGGER, "__control_notify__ctlJoy result: %d", static_cast<int>(result));
        }

        // upNavDataAndFly
        void DafUserControl::__control_notify__navFly(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            mp_log_info(DEFAULT_LOGGER, "__control_notify__navFly Mission Start");
            using std::chrono::seconds;
            using std::this_thread::sleep_for;
            // navDataProperty
            XydAgent::XydManager *xyd_manager = XydAgent::XydManager::getInstance();
            auto xyd_mission = xyd_manager->m_xyd_mission;
            auto xyd_action = xyd_manager->m_xyd_action;
            auto xyd_telemetry = xyd_manager->m_telemetry;

            std::vector<Mission::MissionItem> mission_items;
            Mission::MissionPlan mission_plan{};

            // 解析cmd 构建 mavlink mission 对象
            for (const auto &item : cmd.property_array().properties())
            {
                // auto index = item.property_struct().members().at("index").property_int().value();
                auto propertys = item.property_struct().members();

                const auto longitude = propertys.at("longitude").property_double().value();
                const auto latitude = propertys.at("latitude").property_double().value();
                const auto altitude = propertys.at("altitude").property_float().value();
                const auto speed = propertys.at("speed").property_float().value();
                const auto loiter_time_s = propertys.at("hold_time").property_int().value();

                auto mission_item = XydAgent::XydManager::make_mission_item(
                    latitude, longitude, altitude, speed, false, static_cast<float>(loiter_time_s));
                mp_log_info(
                    DEFAULT_LOGGER,
                    "__control_notify__navFly Mission Receiving Waypoint lat: %f, lon: %f, alt: %f, speed: %f, hold_time: %d",
                    latitude, longitude, altitude, speed, loiter_time_s);

                mission_items.push_back(mission_item);
            }
            mission_plan.mission_items = mission_items;
            if (const Mission::Result upload_result = xyd_mission->upload_mission(mission_plan); upload_result !=
                                                                                                 Mission::Result::Success)
            {
                mp_log_info(DEFAULT_LOGGER, "__control_notify__navFly Mission Upload Failed");
                return;
            }

            mp_log_info(DEFAULT_LOGGER, "__control_notify__navFly Mission Upload Success");

            const Action::Result arm_result = xyd_action->arm();
            if (arm_result != Action::Result::Success)
            {
                mp_log_info(DEFAULT_LOGGER, "Arming failed: %d", static_cast<int>(arm_result));
                return;
            }
            mp_log_info(DEFAULT_LOGGER, "__control_notify__navFly Mission Arm success result: %d",
                        static_cast<int>(arm_result));

            sleep_for(std::chrono::milliseconds(250));

            Mission::Result start_mission_result = xyd_mission->start_mission();
            if (start_mission_result != Mission::Result::Success)
            {
                mp_log_info(DEFAULT_LOGGER, "__control_notify__navFly Mission Starting mission failed: %d",
                            static_cast<int>(start_mission_result));
                return;
            }
            mp_log_info(DEFAULT_LOGGER, "__control_notify__navFly Mission Starting mission success: %d",
                        static_cast<int>(start_mission_result));

            while (!xyd_mission->is_mission_finished().second)
            {
                sleep_for(seconds(1));
            }

            mp_log_info(DEFAULT_LOGGER, "__control_notify__navFly Mission Commanding Hold...");
            sleep_for(std::chrono::milliseconds(250));
            const Action::Result hold_result = xyd_action->hold();
            if (hold_result != Action::Result::Success)
            {
                mp_log_info(DEFAULT_LOGGER, "__control_notify__navFly Mission Failed to command Hold: %d",
                            static_cast<int>(hold_result));
            }

            mp_log_info(DEFAULT_LOGGER, "__control_notify__navFly Mission Commanded hold_result...");

            mp_log_info(DEFAULT_LOGGER, "__control_notify__navFly Mission Done");
        }

        // resumeNav
        void DafUserControl::__control_notify__resumeNav(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // stopNav
        void DafUserControl::__control_notify__stopNav(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // takePhoto
        void DafUserControl::__control_notify__takePhoto(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // startVideo
        void DafUserControl::__control_notify__startVideo(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // stopVideo
        void DafUserControl::__control_notify__stopVideo(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // zoomIn
        void DafUserControl::__control_notify__zoomIn(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // zoomOut
        void DafUserControl::__control_notify__zoomOut(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // zoomScale
        void DafUserControl::__control_notify__zoomScale(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // gimbalFollow
        void DafUserControl::__control_notify__gimbalFollow(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // gimbalReset
        void DafUserControl::__control_notify__gimbalReset(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // gimbalFreeCtl
        void DafUserControl::__control_notify__gimbalFree(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // gimbalLeftCtl
        void DafUserControl::__control_notify__gimbalLeft(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // gimbalRightCtl
        void DafUserControl::__control_notify__gimbalRight(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // gimbalUpCtl
        void DafUserControl::__control_notify__gimbalUp(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // gimbalDownCtl
        void DafUserControl::__control_notify__gimbalDown(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // weaponInsuranceSwitch
        void DafUserControl::__control_notify__weaponSwitch(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // singleShot
        void DafUserControl::__control_notify__sShot(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // runningShot
        void DafUserControl::__control_notify__rShot(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // bomb
        void DafUserControl::__control_notify__bomb(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // suicideAttack
        void DafUserControl::__control_notify__attack(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            // user code
        }

        // attackTargetId
        void DafUserControl::__control_notify__attackTar(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            mp_log_info(DEFAULT_LOGGER, "DafUserControl::__control_notify__attackTar Start");
            std::string targetId = cmd.property_struct().members().at("attackTar").property_string().value();
            ConnectManager::getInstance()->AutoCmdClient.send_message(targetId);
        }

        // stopAutoMode
        void DafUserControl::__control_notify__stopAuto(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            mp_log_info(DEFAULT_LOGGER, "DafUserControl::__control_notify__stopAuto Start");
            std::string Mode = "stopAuto";
            ConnectManager::getInstance()->AutoCmdClient.send_message(Mode);
        }

        // startAutoMode
        void DafUserControl::__control_notify__startAuto(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            mp_log_info(DEFAULT_LOGGER, "DafUserControl::__control_notify__startAuto Start");
            std::string Mode = "startAuto";
            ConnectManager::getInstance()->AutoCmdClient.send_message(Mode);
        }

        // restartFlyCtrl
        void DafUserControl::__control_notify__restartFly(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            mp_log_info(DEFAULT_LOGGER, "DafUserControl::__control_notify__restartFly Start");
            Action::Result restart_fly_ctrl_result = XydAgent::XydManager::getInstance()->m_xyd_action->reboot();
            mp_log_info(DEFAULT_LOGGER, "DafUserControl::__control_notify__restartFly result: %d",
                        static_cast<int>(restart_fly_ctrl_result));
        }

        // stopFlyCtrl
        void DafUserControl::__control_notify__stopFly(const DeviceAgentModel::DeviceAgentProperty &cmd, DeviceAgentModel::DeviceAgentProperty &ack)
        {
            mp_log_info(DEFAULT_LOGGER, "DafUserControl::__control_notify__stopFly Start");
            Action::Result close_fly_ctrl = XydAgent::XydManager::getInstance()->m_xyd_action->shutdown();
            mp_log_info(DEFAULT_LOGGER, "DafUserControl::__control_notify__stopFly result: %d",
                        static_cast<int>(close_fly_ctrl));
        }

    }
}